﻿using System;
using Microsoft.SPOT;

using GT = Gadgeteer;
using GTM = Gadgeteer.Modules;
using GTI = Gadgeteer.Interfaces;

namespace Gadgeteer.Modules.MSR
{
    /// <summary>
    /// Represents a hardware module that can detect acceleration through three axes.
    /// </summary>
    /// <remarks>
    /// <para>
    ///  The hardware that <see cref="Accelerometer"/> represents is a digital high-performance tri-axial low-g acceleration sensor.
    ///  It enables you to perform static and dynamic measurements with high accuracy. You can use an <see cref="Accelerometer"/> for
    ///  tilt, motion and vibration sensing in:
    /// </para>
    /// <list type="bullet">
    ///  <item>Navigation devices</item>
    ///  <item>Robotics</item>
    ///  <item>Gesture recognition</item>
    ///  <item>E-Compass</item>
    ///  <item>Cell Phones</item>
    ///  <item>Digital cameras</item>
    /// </list>
    /// </remarks>
    /// <example>
    /// <para>The following example shows initialization of an accelerometer module and the event handler that registers high acceleration.
    /// High acceleration raises the event handler and the parameters contain information about the sender, the axis of acceleration, and 
    /// direction of acceleration.
    ///</para>
    /// <code>
    ///using Microsoft.SPOT;

    ///using GTM = Gadgeteer.Modules;

    ///namespace Accelerometer
    ///{
    ///    public partial class Program
    ///    {
    ///        // This method is run when the mainboard is powered up or reset.   
    ///        void ProgramStarted()
    ///        {
    ///            accelerometer.HighAccelerationDetected +=
    ///                new GTM.MSR.Accelerometer.HighAccelerationDetectedEventHandler(accelerometer_HighAccelerationDetected);

    ///            // Use Debug.Print to show messages in Visual Studio's "Output" window during debugging.
    ///            Debug.Print("Program Started");
    ///        }

    ///        void accelerometer_HighAccelerationDetected(GTM.MSR.Accelerometer sender, GTM.MSR.Accelerometer.Axis axis,
    ///                                                                                GTM.MSR.Accelerometer.Direction direction)
    ///        {
    ///            accelerometer.MeasurementComplete +=
    ///                new GTM.MSR.Accelerometer.MeasurementCompleteEventHandler(accelerometer_MeasurementComplete);
    ///            Debug.Print("High acceleration detected on: " + " axis in the " + direction + " direction.");
    ///        }

    ///        void accelerometer_MeasurementComplete(GTM.MSR.Accelerometer sender, GTM.MSR.Accelerometer.Acceleration acceleration)
    ///        {
    ///            Debug.Print(sender + " measurement complete.");
    ///        }
    ///    }
    ///}
    ///
    /// </code>
    /// </example>
    public class Accelerometer : GTM.Module
    {
        private GTI.I2CBus i2c;

        private GTI.InterruptInput _interruptPin;

        private GT.Timer _continuousTimer;

        // Note: A constructor summary is auto-generated by the doc builder.
        /// <summary></summary>
        /// <param name="socketNumber">The mainboard socket that has the <see cref="Accelerometer"/> plugged into it.</param>
        public Accelerometer(int socketNumber)
        {
            Socket socket = Socket.GetSocket(socketNumber, true, this, null);

            _interruptPin = new GTI.InterruptInput(socket, Socket.Pin.Three, GTI.GlitchFilterMode.On, GTI.ResistorMode.Disabled, GTI.InterruptMode.RisingEdge, this);
            _interruptPin.Interrupt += new GTI.InterruptInput.InterruptEventHandler(_interruptPin_Interrupt);

            i2c = new GTI.I2CBus(socket, 0x40, 100, this);

            byte[] verdata = new byte[2];
            i2c.WriteRead(new byte[] { 0x00 }, verdata, 1000);
            if (verdata[0] != 0x03 || verdata[1] != 0x14)
            {
                ErrorPrint("Accelerometer module chip id or version not correct");
                throw new Exception("Accelerometer module chip id or version not correct");
            }

            byte[] reg = new byte[1];

            // enable writing to RAM
            var ctrl_reg_0_write = new byte[] { 0x0D, 0x10 };
            i2c.Write(ctrl_reg_0_write, 1000);

            // write 0 to mode_config
            var tco_z_reg = new byte[] { 0x30 };
            i2c.WriteRead(tco_z_reg, reg, 1000);
            reg[0] = (byte)((reg[0] & 0xFC) + 0x0);
            i2c.Write(new byte[] { tco_z_reg[0], reg[0] }, 1000);

            FilterBandwidth = Bandwidth._1200Hz;
            AccelerationRange = Range.SixteenG;

            _continuousTimer = new Gadgeteer.Timer(new TimeSpan(0, 0, 0, 0, 100));
            _continuousTimer.Tick += new Gadgeteer.Timer.TickEventHandler(_continuousTimer_Tick);
        }

        void _continuousTimer_Tick(Gadgeteer.Timer timer)
        {
            if (!_continuousMeasurement)
            {
                timer.Stop();
                return;
            }
            RequestMeasurement();
        }


        byte[] status_reg_3 = new byte[] { 0x0B };
        byte[] intbuf = new byte[2];
        void _interruptPin_Interrupt(GTI.InterruptInput sender, bool value)
        {
            i2c.WriteRead(status_reg_3, intbuf, 1000);
            if ((intbuf[0] & 0x04) > 0)
            {
                OnHighAccelerationDetectedEvent(this, Axis.X, (intbuf[1] & 0x80) > 0 ? Direction.Positive : Direction.Negative);
                return;
            }
            if ((intbuf[0] & 0x02) > 0)
            {
                OnHighAccelerationDetectedEvent(this, Axis.Y, (intbuf[1] & 0x40) > 0 ? Direction.Positive : Direction.Negative);
                return;
            }
            if ((intbuf[0] & 0x01) > 0)
            {
                OnHighAccelerationDetectedEvent(this, Axis.Z, (intbuf[1] & 0x20) > 0 ? Direction.Positive : Direction.Negative);
                return;
            }
            ErrorPrint("Interrupted but not axis bit set");
            return;
        }

        /// <summary>
        /// Sets up an interrupt on high G movement.
        /// </summary>
        /// <param name="axis">The axis along which to enable high acceleration detection.</param>
        /// <param name="thresholdZeroToOne">
        ///  The threshold expressed as a number in the range 0-1. 
        ///  This value is multiplied by <see cref="AccelerationRange"/> to obtain the threshold in G.
        /// </param>
        /// <param name="duration">The interrupt duration.</param>
        public void EnableHighAccelerationDetection(Axis axis, double thresholdZeroToOne, TimeSpan duration)
        {
            if (thresholdZeroToOne == 0)
            {
                // DO THIS LAST since it enables the interrupt
                // enable high_int and lat_int (latches interrupts)
                var ctrl_reg_3_disable = new byte[] { 0x21, 0x0 };
                i2c.Write(ctrl_reg_3_disable, 1000);
            }

            // write high_threshold 
            byte thresholdByte = (byte)(thresholdZeroToOne * 255);
            var high_th_reg_write = new byte[] { 0x2A, thresholdByte };
            i2c.Write(high_th_reg_write, 1000);

            byte high_int_axis = 0;

            if (axis == Axis.X)
                high_int_axis = 0x80;
            else if (axis == Axis.Y)
                high_int_axis = 0x40;
            else if (axis == Axis.Z)
                high_int_axis = 0x20;
            else if (axis == Axis.All)
                high_int_axis = 0xE0;

            // set high interrupt axes = all
            var high_low_info_write = new byte[] { 0x25, high_int_axis };
            i2c.Write(high_low_info_write, 1000);

            // set interrupt hystersis
            var hy_write = new byte[] { 0x23, 0 };
            i2c.Write(hy_write, 1000);

            // set interrupt duration
            //"Timings used for high acceleration and low acceleration de-bouncing are
            // absolute values (1 LSB of high_dur and low_dur registers corresponds to 5*Tupdate
            // (= 2.085ms)"
            // Default value for high_dur = 0x32
            // Default duration = 2ms * 50 = 100ms?

            byte high_dur = (byte)(duration.Milliseconds / 2);
            var high_dur_write = new byte[] { 0x27, (byte)(high_dur << 1) };
            i2c.Write(high_dur_write, 1000);

            // enable high_int
            var ctrl_reg_3_write = new byte[] { 0x21, 0x20 };
            i2c.Write(ctrl_reg_3_write, 1000);
        }

        /// <summary>
        /// Represents an acceleration measurement.
        /// </summary>
        public class Acceleration
        {
            /// <summary>
            /// Gets the amount of acceleration along the X axis.
            /// </summary>
            public double X { get; private set; }

            /// <summary>
            /// Gets the amount of acceleration along the Y axis.
            /// </summary>
            public double Y { get; private set; }

            /// <summary>
            /// Gets the amount of acceleration along the Z axis.
            /// </summary>
            public double Z { get; private set; }

            // Note: A constructor summary is auto-generated by the doc builder.
            /// <summary></summary>
            /// <param name="X">The amount of acceleration along the X axis.</param>
            /// <param name="Y">The amount of acceleration along the Y axis.</param>
            /// <param name="Z">The amount of acceleration along the Z axis.</param>
            public Acceleration(double X, double Y, double Z)
            {
                this.X = X;
                this.Y = Y;
                this.Z = Z;
            }

            /// <summary>
            /// Gets the magnitude squared, that is: the sum of <see cref="X"/>^2, <see cref="Y"/>^2 and <see cref="Z"/>^2.
            /// </summary>
            public double MagnitudeSquared
            {
                get
                {
                    return X * X + Y * Y + Z * Z;
                }
            }

            /// <summary>
            /// Provides a string representation of the <see cref="Acceleration"/> instance.
            /// </summary>
            /// <returns>A string describing the values contained in the object.</returns>
            public override string ToString()
            {
                return "Mag: " + MagnitudeSquared.ToString("f2") + " X: " + X.ToString("f2") + " Y: " + Y.ToString("f2") + " Z: " + Z.ToString("f2");
            }
        }

        /// <summary>
        /// Provides an enumeration of values that may be assigned to the <see cref="FilterBandwidth"/> property.
        /// </summary>
        public enum Bandwidth : byte
        {
            /// <summary>
            /// 10 Hz low pass filter.
            /// </summary>
            _10Hz = 0x0,
            /// <summary>
            /// 20 Hz low pass filter.
            /// </summary>
            _20Hz = 0x1,
            /// <summary>
            /// 40 Hz low pass filter.
            /// </summary>
            _40Hz = 0x2,
            /// <summary>
            /// 75 Hz low pass filter.
            /// </summary>
            _75Hz = 0x3,
            /// <summary>
            /// 150 Hz low pass filter.
            /// </summary>
            _150Hz = 0x4,
            /// <summary>
            /// 300 Hz low pass filter.
            /// </summary>
            _300Hz = 0x5,
            /// <summary>
            /// 600 Hz low pass filter.
            /// </summary>
            _600Hz = 0x6,
            /// <summary>
            /// 1200 Hz low pass filter.
            /// </summary>
            _1200Hz = 0x7,
            /// <summary>
            /// 1 Hz high pass filter.
            /// </summary>
            HighPass1Hz = 0x8,
            /// <summary>
            /// 0.2 - 300 Hz band pass filter.
            /// </summary>
            BandPass0_2HzTo300Hz = 0x9
        }

        /// <summary>
        ///  Provides an enumeration of values that may be assigned to the <see cref="AccelerationRange"/> property.
        /// </summary>
        public enum Range : byte
        {
            /// <summary>
            /// 1 G.
            /// </summary>
            OneG = 0x00,
            /// <summary>
            /// 1.5 G.
            /// </summary>
            OnePointFiveG = 0x01,
            /// <summary>
            /// 2 G.
            /// </summary>
            TwoG = 0x02,
            /// <summary>
            /// 3 G.
            /// </summary>
            ThreeG = 0x03,
            /// <summary>
            /// 4 G.
            /// </summary>
            FourG = 0x04,
            /// <summary>
            /// 8 G.
            /// </summary>
            EightG = 0x05,
            /// <summary>
            /// 16 G.
            /// </summary>
            SixteenG = 0x06
        }

        private static double ConvertRangeToGPerBit(Range r)
        {
            switch (r)
            {
                case Range.OneG: return 0.00013;
                case Range.OnePointFiveG: return 0.00019;
                case Range.TwoG: return 0.00025;
                case Range.ThreeG: return 0.00038;
                case Range.FourG: return 0.00050;
                case Range.EightG: return 0.00099;
                case Range.SixteenG: return 0.00198;
                default: throw new ArgumentOutOfRangeException();
            }
        }

        byte[] offset_lsb_1 = new byte[] { 0x35 };
        byte[] ARreg = new byte[1];
        byte[] ARregw = new byte[2] { 0x35, 0 };
        private Range _range = Range.OneG;

        /// <summary>
        /// Gets or sets the range of acceleration.
        /// </summary>
        /// <remarks>
        /// The default value for this property is <see cref="Range">Range.OneG</see>.
        /// </remarks>
        public Range AccelerationRange
        {
            get
            {
                i2c.WriteRead(offset_lsb_1, ARreg, 1000);
                Range res = (Range)((ARreg[0] & 0x0E) >> 1);
                if (_range != res)
                {
                    ErrorPrint("Range read back wrong - is " + res + " expecting " + _range);
                    _range = res;
                }
                return _range;
            }

            set
            {
                i2c.WriteRead(offset_lsb_1, ARreg, 1000);
                ARregw[1] = (byte)((ARreg[0] & 0xF1) + ((byte)value << 1));
                i2c.Write(ARregw, 1000);
                _range = value;
            }

        }


        private Bandwidth _bandwidth = Bandwidth._1200Hz;
        byte[] bw_tcs_reg = new byte[] { 0x20 };
        byte[] BWreg = new byte[1];
        byte[] BWregw = new byte[2] { 0x20, 0 };

        /// <summary>
        /// Gets or sets the filter bandwidth.
        /// </summary>
        /// <remarks>
        /// The default value for this property is <see cref="Bandwidth">Bandwidth._1200Hz</see>.
        /// </remarks>
        public Bandwidth FilterBandwidth
        {
            get
            {
                return _bandwidth;
            }
            set
            {
                i2c.WriteRead(bw_tcs_reg, BWreg, 1000);
                BWregw[1] = (byte)((BWreg[0] & 0x0F) + (byte)value);
                i2c.Write(BWregw, 1000);
                _bandwidth = value;
            }
        }

        /// <summary>
        /// Gets or sets the interval at which continuous measurements are taken.
        /// </summary>
        /// <remarks>
        /// The default value for this property is 100 milliseconds.
        /// </remarks>
        public TimeSpan ContinuousMeasurementInterval
        {
            get
            {
                return _continuousTimer.Interval;
            }
            set
            {
                _continuousTimer.Stop();
                _continuousTimer.Interval = value;
                if (_continuousMeasurement) _continuousTimer.Start();
            }

        }

        /// <summary>
        /// Obtains a reading from the <see cref="Accelerometer"/> and raises the <see cref="MeasurementComplete"/> event when complete.
        /// </summary>
        /// <returns>This method always returns <b>true</b>.</returns>
        public bool RequestMeasurement()
        {
            byte[] recvData = new byte[6];
            i2c.WriteRead(new byte[] { 0x02 }, recvData, 1000);
            //string s = "Accel data: ";
            //foreach (byte b in recvData) s += Gadgeteer.MeshRadio.Utility.HexPrint(b) + " ";

            double X = ((double)((((recvData[1] & 0x7F) + (((recvData[1] & 0x80) > 0) ? -128 : 0)) << 6) + (recvData[0] >> 2))) * ConvertRangeToGPerBit(AccelerationRange);
            double Y = ((double)((((recvData[3] & 0x7F) + (((recvData[3] & 0x80) > 0) ? -128 : 0)) << 6) + (recvData[2] >> 2))) * ConvertRangeToGPerBit(AccelerationRange);
            double Z = ((double)((((recvData[5] & 0x7F) + (((recvData[5] & 0x80) > 0) ? -128 : 0)) << 6) + (recvData[4] >> 2))) * ConvertRangeToGPerBit(AccelerationRange);
            Acceleration a = new Acceleration(X, Y, Z);

            //Debug.Print(a.ToString());

            OnMeasurementCompleteEvent(this, a);
            return true;
        }

        private bool _continuousMeasurement = false;

        /// <summary>
        /// Starts continuous measurements.
        /// </summary>
        /// <remarks>
        /// When this method is called, <see cref="Accelerometer"/> begins taking continuous measurements.
        /// At each <see cref="ContinuousMeasurementInterval"/>, it calls the <see cref="RequestMeasurement"/> method,
        /// which raises the <see cref="MeasurementComplete"/> event.
        /// </remarks>
        public void StartContinuousMeasurements()
        {
            _continuousMeasurement = true;
            _continuousTimer.Start();
        }

        /// <summary>
        /// Stops continuous measurements.
        /// </summary>
        public void StopContinuousMeasurements()
        {
            _continuousMeasurement = false;
            _continuousTimer.Stop();
        }

        /// <summary>
        /// Represents the delegate used for the <see cref="MeasurementComplete"/> event.
        /// </summary>
        /// <param name="sender">The object that raised the event.</param>
        /// <param name="acceleration">The <see cref="Acceleration"/> object that contains the results of the reading.</param>
        public delegate void MeasurementCompleteEventHandler(Accelerometer sender, Acceleration acceleration);

        /// <summary>
        /// Event raised when a measurement reading is completed.
        /// </summary>
        public event MeasurementCompleteEventHandler MeasurementComplete;

        private MeasurementCompleteEventHandler _OnMeasurementComplete;

        /// <summary>
        /// Raises the <see cref="MeasurementComplete"/> event.
        /// </summary>
        /// <param name="sender">The object that raised the event.</param>
        /// <param name="acceleration">The <see cref="Acceleration"/> object that contains the results of the reading.</param>
        protected virtual void OnMeasurementCompleteEvent(Accelerometer sender, Acceleration acceleration)
        {
            if (_OnMeasurementComplete == null) _OnMeasurementComplete = new MeasurementCompleteEventHandler(OnMeasurementCompleteEvent);
            if (Program.CheckAndInvoke(MeasurementComplete, _OnMeasurementComplete, sender, acceleration))
            {
                MeasurementComplete(sender, acceleration);
            }
        }

        /// <summary>
        /// The axis along which acceleration occurs.
        /// </summary>
        public enum Axis
        {
            /// <summary>
            /// Acceleration along the X axis.
            /// </summary>
            X,
            /// <summary>
            /// Acceleration along the Y axis.
            /// </summary>
            Y,
            /// <summary>
            /// Acceleration along the Z axis.
            /// </summary>
            Z,
            /// <summary>
            /// Acceleration along the X, Y and Z axes.
            /// </summary>
            All
        }

        /// <summary>
        /// The direction of acceleration.
        /// </summary>
        public enum Direction
        {
            /// <summary>
            /// Acceleration is in a positive direction.
            /// </summary>
            Positive,
            /// <summary>
            /// Acceleration is in a negative direction.
            /// </summary>
            Negative
        }


        /// <summary>
        /// Represents the delegate used for the <see cref="HighAccelerationDetected"/> event.
        /// </summary>
        /// <param name="sender">The object that raised the event.</param>
        /// <param name="axis">The axis of the acceleration.</param>
        /// <param name="direction">The direction of the acceleration.</param>
        public delegate void HighAccelerationDetectedEventHandler(Accelerometer sender, Axis axis, Direction direction);

        /// <summary>
        /// Event raised when high acceleration is detected.
        /// </summary>
        public event HighAccelerationDetectedEventHandler HighAccelerationDetected;

        private HighAccelerationDetectedEventHandler _OnHighAccelerationDetected;

        /// <summary>
        /// Raises the <see cref="HighAccelerationDetected"/> event.
        /// </summary>
        /// <param name="sender">The object that raised the event.</param>
        /// <param name="axis">The axis of the acceleration.</param>
        /// <param name="direction">The direction of the acceleration.</param>
        protected virtual void OnHighAccelerationDetectedEvent(Accelerometer sender, Axis axis, Direction direction)
        {
            if (_OnHighAccelerationDetected == null) _OnHighAccelerationDetected = new HighAccelerationDetectedEventHandler(OnHighAccelerationDetectedEvent);
            if (Program.CheckAndInvoke(HighAccelerationDetected, _OnHighAccelerationDetected, sender, axis, direction))
            {
                HighAccelerationDetected(sender, axis, direction);
            }
        }

    } 
}
